import time
from typing import Tuple
import serial

SERIAL_COM_MOTOR = 'COM3'  # 在windows系统，需要在Linux中修改
SERIAL_COM_SERVO = 'COM4'  # 舵机串口
PICTURE_SCALE = 100  # 根据实际修改
HEIGHT = 10  # 以毫米为单位Z轴高低
TARGET = ((1, 1, 1), (2, 2, 2), (3, 3, 3), (4, 4, 4))
# 四个目标的地址


def gcode(xy: Tuple[float, float, float, float], target: int):  # xy应该为(x1,y1,x2,y2)
    x = (xy[0] + xy[1]) / (2 * PICTURE_SCALE)
    y = (xy[2] + xy[3]) / (2 * PICTURE_SCALE)
    g_code_s = ('G21', f"G1 X{x} Y{y}", f"Z{HEIGHT}")  # G21表示以毫米为单位
    g_code_t = ('G21', 'G1 Z0', f"G1 X{TARGET[target][0]} Y{TARGET[target][1]}", f"Z{TARGET[target][2]}")
    g_code_r = ('G21', 'G1 X0 Y0 Z0')
    return g_code_r, g_code_t, g_code_s


def flowing_send(xy: Tuple[float, float, float, float], target: int):
    motor = serial.Serial(SERIAL_COM_MOTOR, 115200)
    servo = serial.Serial(SERIAL_COM_SERVO, 9600)
    motor.write(b"\r\n\r\n")
    time.sleep(1)
    g_code_r, g_code_t, g_code_s = gcode(xy, target)
    motor.flush()
    for command in g_code_s:
        print(f'Sending: {command}')
        motor.write(command.join('\n').encode())
        grbl_out = motor.readline()
        print(f'response: {grbl_out}')
    servo.write('open grap'.encode())
    servoout = servo.readline()
    print(servoout)
    if servoout == 'ok':
        for command in g_code_t:
            print(f'Sending: {command}')
            motor.write(command.join('\n').encode())
            grbl_out = motor.readline()
            print(f'response: {grbl_out}')
    else:
        print('Do not receive ok command')
    servo.write(f'open{target}'.encode())
    servo_open = servo.readline()
    if servo_open == 'ok':
        for command in g_code_r:
            print(f'Sending: {command}')
            motor.write(command.join('\n').encode())
            grbl_out = motor.readline()
            print(f'response: {grbl_out}')
    else:
        print('Do not receive return command')
    motor.close()
    servo.close()

